#include "CQuadrotor.h"

CQuadrotor::CQuadrotor() {
	timestamp=timestampToDouble(now()); // ajouté pour ajuster le dt
	model.init();
}

CQuadrotor::~CQuadrotor() {

}

void CQuadrotor::init() {
	model.init();
}

CPose3D CQuadrotor::getRenderPose() {
	return model.getRenderPose();
}

void CQuadrotor::simulate() {
	vector<double> pose_noise(3);
	double maintenant=timestampToDouble(now());
	//vector<double> v_noise(3);
	vector<double> scan_noise(481);
	vector<double> kill_obs(481);
	mrpt::random::vectorRandomUni(scan_noise,0.99,1.01);//erreur de 1%
	mrpt::random::vectorRandomUni(kill_obs,0.0,100.0);//perte des scan.

	//mrpt::random::vectorRandomNormal(v_noise, 0.0, 0.3);
	//mrpt::random::vectorRandomNormal(pose_noise, 0.0, 0.1);
	//CPose3D tt(model.m_pose);
	//tt.x(tt.x() + pose_noise[0]);
	//tt.y(tt.y() + pose_noise[1]);
	//tt.z(tt.z() + pose_noise[2]);
	if(maintenant-timestamp>=0.1) // un scan tous les 200ms
	//if(true)
	{
		laser.simulateLaserScan(view);
		for(int i=0;i<481;i++){
				laser.scan_obs.scan[i]*=(kill_obs[i]<20.0)?0:1;
				laser.scan_obs.scan[i]*=scan_noise[i];
		}
		pilot.estimation(laser.scan_obs, model.m_pose);
		timestamp=maintenant;
		//printf("ax=%f,ay=%f,az=%f",model.acc[1],model.acc[2],model.acc[3]);
	}

	//pilot.piloteHuit(laser.scan_obs, tt, TPoint3D(model.getVx() + v_noise[0],model.getVy() + v_noise[1], 0));
	//pilot.piloteWpt(model.m_pose,TPoint3D(model.getVx(),model.getVy(),0));
	pilot.piloteHuit(laser.scan_obs,model.m_pose,TPoint3D(model.getVx(),model.getVy(),0));

	model.SetCommands(pilot.getPhiC(), pilot.getThetaC(), pilot.getPsiC(), -2);
	model.simulate();
	laser.setPose(model.getRenderPose());
	//pilot.autoPilot.param8.setParamHuit((float) ((int) rand()*100)/100.0,(float) ((int) rand()*100)/100.0,(float) ((int) rand()*100)/100.0, (float) ((int) rand()*100)/100.0, (float) ((int) rand()*100)/100.0,(float) ((int) rand()*100)/100.0,(float) ((int) rand()*100)/100.0);
	//pilot.autoPilot.paramE.setParamEstim((float) ((int) rand()*100)/100.0,(float) ((int) rand()*100)/100.0,(float) ((int) rand()*100)/100.0, (float) ((int) rand()*100)/100.0,(float) ((int) rand()*100)/100.0, (float) ((int) rand()*100)/100.0);
}

void CQuadrotor::setView(COpenGLViewportPtr view_) {
	view = view_;
}
